home *** CD-ROM | disk | FTP | other *** search
/ io Programmo 60 / IOPROG_60.ISO / soft / c++ / gsl-1.1.1-setup.exe / {app} / src / multiroots / hybridj.c < prev    next >
Encoding:
C/C++ Source or Header  |  2001-08-22  |  14.4 KB  |  609 lines
  1. /* multiroots/hybridj.c
  2.  * 
  3.  * Copyright (C) 1996, 1997, 1998, 1999, 2000 Brian Gough
  4.  * 
  5.  * This program is free software; you can redistribute it and/or modify
  6.  * it under the terms of the GNU General Public License as published by
  7.  * the Free Software Foundation; either version 2 of the License, or (at
  8.  * your option) any later version.
  9.  * 
  10.  * This program is distributed in the hope that it will be useful, but
  11.  * WITHOUT ANY WARRANTY; without even the implied warranty of
  12.  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  13.  * General Public License for more details.
  14.  * 
  15.  * You should have received a copy of the GNU General Public License
  16.  * along with this program; if not, write to the Free Software
  17.  * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  18.  */
  19.  
  20. #include <config.h>
  21.  
  22. #include <stddef.h>
  23. #include <stdlib.h>
  24. #include <stdio.h>
  25. #include <math.h>
  26. #include <float.h>
  27.  
  28. #include <gsl/gsl_math.h>
  29. #include <gsl/gsl_errno.h>
  30. #include <gsl/gsl_multiroots.h>
  31. #include <gsl/gsl_linalg.h>
  32.  
  33. #include "dogleg.c"
  34.  
  35. typedef struct
  36.   {
  37.     size_t iter;
  38.     size_t ncfail;
  39.     size_t ncsuc;
  40.     size_t nslow1;
  41.     size_t nslow2;
  42.     double fnorm;
  43.     double delta;
  44.     gsl_matrix *q;
  45.     gsl_matrix *r;
  46.     gsl_vector *tau;
  47.     gsl_vector *diag;
  48.     gsl_vector *qtf;
  49.     gsl_vector *newton;
  50.     gsl_vector *gradient;
  51.     gsl_vector *x_trial;
  52.     gsl_vector *f_trial;
  53.     gsl_vector *df;
  54.     gsl_vector *qtdf;
  55.     gsl_vector *rdx;
  56.     gsl_vector *w;
  57.     gsl_vector *v;
  58.   }
  59. hybridj_state_t;
  60.  
  61. static int hybridj_alloc (void *vstate, size_t n);
  62. static int hybridj_set (void *vstate, gsl_multiroot_function_fdf * fdf, gsl_vector * x, gsl_vector * f, gsl_matrix * J, gsl_vector * dx);
  63. static int hybridsj_set (void *vstate, gsl_multiroot_function_fdf * fdf, gsl_vector * x, gsl_vector * f, gsl_matrix * J, gsl_vector * dx);
  64. static int set (void *vstate, gsl_multiroot_function_fdf * fdf, gsl_vector * x, gsl_vector * f, gsl_matrix * J, gsl_vector * dx, int scale);
  65. static int hybridj_iterate (void *vstate, gsl_multiroot_function_fdf * fdf, gsl_vector * x, gsl_vector * f, gsl_matrix * J, gsl_vector * dx);
  66. static void hybridj_free (void *vstate);
  67. static int iterate (void *vstate, gsl_multiroot_function_fdf * fdf, gsl_vector * x, gsl_vector * f, gsl_matrix * J, gsl_vector * dx, int scale);
  68.  
  69. static int
  70. hybridj_alloc (void *vstate, size_t n)
  71. {
  72.   hybridj_state_t *state = (hybridj_state_t *) vstate;
  73.   gsl_matrix *q, *r;
  74.   gsl_vector *tau, *diag, *qtf, *newton, *gradient, *x_trial, *f_trial,
  75.    *df, *qtdf, *rdx, *w, *v;
  76.  
  77.   q = gsl_matrix_calloc (n, n);
  78.  
  79.   if (q == 0)
  80.     {
  81.       GSL_ERROR_VAL ("failed to allocate space for q", GSL_ENOMEM, 0);
  82.     }
  83.  
  84.   state->q = q;
  85.  
  86.   r = gsl_matrix_calloc (n, n);
  87.  
  88.   if (r == 0)
  89.     {
  90.       gsl_matrix_free (q);
  91.  
  92.       GSL_ERROR_VAL ("failed to allocate space for r", GSL_ENOMEM, 0);
  93.     }
  94.  
  95.   state->r = r;
  96.  
  97.   tau = gsl_vector_calloc (n);
  98.  
  99.   if (tau == 0)
  100.     {
  101.       gsl_matrix_free (q);
  102.       gsl_matrix_free (r);
  103.  
  104.       GSL_ERROR_VAL ("failed to allocate space for tau", GSL_ENOMEM, 0);
  105.     }
  106.  
  107.   state->tau = tau;
  108.  
  109.   diag = gsl_vector_calloc (n);
  110.  
  111.   if (diag == 0)
  112.     {
  113.       gsl_matrix_free (q);
  114.       gsl_matrix_free (r);
  115.       gsl_vector_free (tau);
  116.  
  117.       GSL_ERROR_VAL ("failed to allocate space for diag", GSL_ENOMEM, 0);
  118.     }
  119.  
  120.   state->diag = diag;
  121.  
  122.   qtf = gsl_vector_calloc (n);
  123.  
  124.   if (qtf == 0)
  125.     {
  126.       gsl_matrix_free (q);
  127.       gsl_matrix_free (r);
  128.       gsl_vector_free (tau);
  129.       gsl_vector_free (diag);
  130.  
  131.       GSL_ERROR_VAL ("failed to allocate space for qtf", GSL_ENOMEM, 0);
  132.     }
  133.  
  134.   state->qtf = qtf;
  135.  
  136.   newton = gsl_vector_calloc (n);
  137.  
  138.   if (newton == 0)
  139.     {
  140.       gsl_matrix_free (q);
  141.       gsl_matrix_free (r);
  142.       gsl_vector_free (tau);
  143.       gsl_vector_free (diag);
  144.       gsl_vector_free (qtf);
  145.  
  146.       GSL_ERROR_VAL ("failed to allocate space for newton", GSL_ENOMEM, 0);
  147.     }
  148.  
  149.   state->newton = newton;
  150.  
  151.   gradient = gsl_vector_calloc (n);
  152.  
  153.   if (gradient == 0)
  154.     {
  155.       gsl_matrix_free (q);
  156.       gsl_matrix_free (r);
  157.       gsl_vector_free (tau);
  158.       gsl_vector_free (diag);
  159.       gsl_vector_free (qtf);
  160.       gsl_vector_free (newton);
  161.  
  162.       GSL_ERROR_VAL ("failed to allocate space for gradient", GSL_ENOMEM, 0);
  163.     }
  164.  
  165.   state->gradient = gradient;
  166.  
  167.   x_trial = gsl_vector_calloc (n);
  168.  
  169.   if (x_trial == 0)
  170.     {
  171.       gsl_matrix_free (q);
  172.       gsl_matrix_free (r);
  173.       gsl_vector_free (tau);
  174.       gsl_vector_free (diag);
  175.       gsl_vector_free (qtf);
  176.       gsl_vector_free (newton);
  177.       gsl_vector_free (gradient);
  178.  
  179.       GSL_ERROR_VAL ("failed to allocate space for x_trial", GSL_ENOMEM, 0);
  180.     }
  181.  
  182.   state->x_trial = x_trial;
  183.  
  184.   f_trial = gsl_vector_calloc (n);
  185.  
  186.   if (f_trial == 0)
  187.     {
  188.       gsl_matrix_free (q);
  189.       gsl_matrix_free (r);
  190.       gsl_vector_free (tau);
  191.       gsl_vector_free (diag);
  192.       gsl_vector_free (qtf);
  193.       gsl_vector_free (newton);
  194.       gsl_vector_free (gradient);
  195.       gsl_vector_free (x_trial);
  196.  
  197.       GSL_ERROR_VAL ("failed to allocate space for f_trial", GSL_ENOMEM, 0);
  198.     }
  199.  
  200.   state->f_trial = f_trial;
  201.  
  202.   df = gsl_vector_calloc (n);
  203.  
  204.   if (df == 0)
  205.     {
  206.       gsl_matrix_free (q);
  207.       gsl_matrix_free (r);
  208.       gsl_vector_free (tau);
  209.       gsl_vector_free (diag);
  210.       gsl_vector_free (qtf);
  211.       gsl_vector_free (newton);
  212.       gsl_vector_free (gradient);
  213.       gsl_vector_free (x_trial);
  214.       gsl_vector_free (f_trial);
  215.  
  216.       GSL_ERROR_VAL ("failed to allocate space for df", GSL_ENOMEM, 0);
  217.     }
  218.  
  219.   state->df = df;
  220.  
  221.   qtdf = gsl_vector_calloc (n);
  222.  
  223.   if (qtdf == 0)
  224.     {
  225.       gsl_matrix_free (q);
  226.       gsl_matrix_free (r);
  227.       gsl_vector_free (tau);
  228.       gsl_vector_free (diag);
  229.       gsl_vector_free (qtf);
  230.       gsl_vector_free (newton);
  231.       gsl_vector_free (gradient);
  232.       gsl_vector_free (x_trial);
  233.       gsl_vector_free (f_trial);
  234.       gsl_vector_free (df);
  235.  
  236.       GSL_ERROR_VAL ("failed to allocate space for qtdf", GSL_ENOMEM, 0);
  237.     }
  238.  
  239.   state->qtdf = qtdf;
  240.  
  241.  
  242.   rdx = gsl_vector_calloc (n);
  243.  
  244.   if (rdx == 0)
  245.     {
  246.       gsl_matrix_free (q);
  247.       gsl_matrix_free (r);
  248.       gsl_vector_free (tau);
  249.       gsl_vector_free (diag);
  250.       gsl_vector_free (qtf);
  251.       gsl_vector_free (newton);
  252.       gsl_vector_free (gradient);
  253.       gsl_vector_free (x_trial);
  254.       gsl_vector_free (f_trial);
  255.       gsl_vector_free (df);
  256.       gsl_vector_free (qtdf);
  257.  
  258.       GSL_ERROR_VAL ("failed to allocate space for rdx", GSL_ENOMEM, 0);
  259.     }
  260.  
  261.   state->rdx = rdx;
  262.  
  263.   w = gsl_vector_calloc (n);
  264.  
  265.   if (w == 0)
  266.     {
  267.       gsl_matrix_free (q);
  268.       gsl_matrix_free (r);
  269.       gsl_vector_free (tau);
  270.       gsl_vector_free (diag);
  271.       gsl_vector_free (qtf);
  272.       gsl_vector_free (newton);
  273.       gsl_vector_free (gradient);
  274.       gsl_vector_free (x_trial);
  275.       gsl_vector_free (f_trial);
  276.       gsl_vector_free (df);
  277.       gsl_vector_free (qtdf);
  278.       gsl_vector_free (rdx);
  279.  
  280.       GSL_ERROR_VAL ("failed to allocate space for w", GSL_ENOMEM, 0);
  281.     }
  282.  
  283.   state->w = w;
  284.  
  285.   v = gsl_vector_calloc (n);
  286.  
  287.   if (v == 0)
  288.     {
  289.       gsl_matrix_free (q);
  290.       gsl_matrix_free (r);
  291.       gsl_vector_free (tau);
  292.       gsl_vector_free (diag);
  293.       gsl_vector_free (qtf);
  294.       gsl_vector_free (newton);
  295.       gsl_vector_free (gradient);
  296.       gsl_vector_free (x_trial);
  297.       gsl_vector_free (f_trial);
  298.       gsl_vector_free (df);
  299.       gsl_vector_free (qtdf);
  300.       gsl_vector_free (rdx);
  301.       gsl_vector_free (w);
  302.  
  303.       GSL_ERROR_VAL ("failed to allocate space for v", GSL_ENOMEM, 0);
  304.     }
  305.  
  306.   state->v = v;
  307.  
  308.   return GSL_SUCCESS;
  309. }
  310.  
  311. static int
  312. hybridj_set (void *vstate, gsl_multiroot_function_fdf * fdf, gsl_vector * x, gsl_vector * f, gsl_matrix * J, gsl_vector * dx)
  313. {
  314.   int status = set (vstate, fdf, x, f, J, dx, 0);
  315.   return status ;
  316. }
  317.  
  318. static int
  319. hybridsj_set (void *vstate, gsl_multiroot_function_fdf * fdf, gsl_vector * x, gsl_vector * f, gsl_matrix * J, gsl_vector * dx)
  320. {
  321.   int status = set (vstate, fdf, x, f, J, dx, 1);
  322.   return status ;
  323. }
  324.  
  325. static int
  326. set (void *vstate, gsl_multiroot_function_fdf * fdf, gsl_vector * x, gsl_vector * f, gsl_matrix * J, gsl_vector * dx, int scale)
  327. {
  328.   hybridj_state_t *state = (hybridj_state_t *) vstate;
  329.  
  330.   gsl_matrix *q = state->q;
  331.   gsl_matrix *r = state->r;
  332.   gsl_vector *tau = state->tau;
  333.   gsl_vector *diag = state->diag;
  334.  
  335.   GSL_MULTIROOT_FN_EVAL_F_DF (fdf, x, f, J);
  336.  
  337.   state->iter = 1;
  338.   state->fnorm = enorm (f);
  339.   state->ncfail = 0;
  340.   state->ncsuc = 0;
  341.   state->nslow1 = 0;
  342.   state->nslow2 = 0;
  343.  
  344.   gsl_vector_set_all (dx, 0.0);
  345.  
  346.   /* Store column norms in diag */
  347.  
  348.   if (scale)
  349.     compute_diag (J, diag);
  350.   else
  351.     gsl_vector_set_all (diag, 1.0);
  352.  
  353.   /* Set delta to factor |D x| or to factor if |D x| is zero */
  354.  
  355.   state->delta = compute_delta (diag, x);
  356.  
  357.   /* Factorize J into QR decomposition */
  358.  
  359.   gsl_linalg_QR_decomp (J, tau);
  360.   gsl_linalg_QR_unpack (J, tau, q, r);
  361.  
  362.   return GSL_SUCCESS;
  363. }
  364.  
  365. static int
  366. hybridj_iterate (void *vstate, gsl_multiroot_function_fdf * fdf, gsl_vector * x, gsl_vector * f, gsl_matrix * J, gsl_vector * dx)
  367. {
  368.   int status = iterate (vstate, fdf, x, f, J, dx, 0);
  369.   return status;
  370. }
  371.  
  372. static int
  373. hybridsj_iterate (void *vstate, gsl_multiroot_function_fdf * fdf, gsl_vector * x, gsl_vector * f, gsl_matrix * J, gsl_vector * dx)
  374. {
  375.   int status = iterate (vstate, fdf, x, f, J, dx, 1);
  376.   return status;
  377. }
  378.  
  379. static int
  380. iterate (void *vstate, gsl_multiroot_function_fdf * fdf, gsl_vector * x, gsl_vector * f, gsl_matrix * J, gsl_vector * dx, int scale)
  381. {
  382.   hybridj_state_t *state = (hybridj_state_t *) vstate;
  383.  
  384.   const double fnorm = state->fnorm;
  385.  
  386.   gsl_matrix *q = state->q;
  387.   gsl_matrix *r = state->r;
  388.   gsl_vector *tau = state->tau;
  389.   gsl_vector *diag = state->diag;
  390.   gsl_vector *qtf = state->qtf;
  391.   gsl_vector *x_trial = state->x_trial;
  392.   gsl_vector *f_trial = state->f_trial;
  393.   gsl_vector *df = state->df;
  394.   gsl_vector *qtdf = state->qtdf;
  395.   gsl_vector *rdx = state->rdx;
  396.   gsl_vector *w = state->w;
  397.   gsl_vector *v = state->v;
  398.  
  399.   double prered, actred;
  400.   double pnorm, fnorm1, fnorm1p;
  401.   double ratio;
  402.   double p1 = 0.1, p5 = 0.5, p001 = 0.001, p0001 = 0.0001;
  403.  
  404.   /* Compute qtf = Q^T f */
  405.  
  406.   compute_qtf (q, f, qtf);
  407.  
  408.   /* Compute dogleg step */
  409.  
  410.   dogleg (r, qtf, diag, state->delta, state->newton, state->gradient, dx);
  411.  
  412.   /* Take a trial step */
  413.  
  414.   compute_trial_step (x, dx, state->x_trial);
  415.  
  416.   pnorm = scaled_enorm (diag, dx);
  417.  
  418.   if (state->iter == 1)
  419.     {
  420.       if (pnorm < state->delta)
  421.     {
  422.       state->delta = pnorm;
  423.     }
  424.     }
  425.  
  426.   /* Evaluate function at x + p */
  427.  
  428.   {
  429.     int status = GSL_MULTIROOT_FN_EVAL_F (fdf, x_trial, f_trial);
  430.  
  431.     if (status != GSL_SUCCESS) 
  432.       {
  433.         return GSL_EBADFUNC;
  434.       }
  435.   }
  436.  
  437.   /* Set df = f_trial - f */
  438.  
  439.   compute_df (f_trial, f, df);
  440.  
  441.   /* Compute the scaled actual reduction */
  442.  
  443.   fnorm1 = enorm (f_trial);
  444.  
  445.   actred = compute_actual_reduction (fnorm, fnorm1);
  446.  
  447.   /* Compute rdx = R dx */
  448.  
  449.   compute_rdx (r, dx, rdx);
  450.  
  451.   /* Compute the scaled predicted reduction phi1p = |Q^T f + R dx| */
  452.  
  453.   fnorm1p = enorm_sum (qtf, rdx);
  454.  
  455.   prered = compute_predicted_reduction (fnorm, fnorm1p);
  456.  
  457.   /* Compute the ratio of the actual to predicted reduction */
  458.  
  459.   if (prered > 0)
  460.     {
  461.       ratio = actred / prered;
  462.     }
  463.   else
  464.     {
  465.       ratio = 0;
  466.     }
  467.  
  468.   /* Update the step bound */
  469.  
  470.   if (ratio < p1)
  471.     {
  472.       state->ncsuc = 0;
  473.       state->ncfail++;
  474.       state->delta *= p5;
  475.     }
  476.   else
  477.     {
  478.       state->ncfail = 0;
  479.       state->ncsuc++;
  480.  
  481.       if (ratio >= p5 || state->ncsuc > 1)
  482.     state->delta = GSL_MAX (state->delta, pnorm / p5);
  483.       if (fabs (ratio - 1) <= p1)
  484.     state->delta = pnorm / p5;
  485.     }
  486.  
  487.   /* Test for successful iteration */
  488.  
  489.   if (ratio >= p0001)
  490.     {
  491.       gsl_vector_memcpy (x, x_trial);
  492.       gsl_vector_memcpy (f, f_trial);
  493.       state->fnorm = fnorm1;
  494.       state->iter++;
  495.     }
  496.  
  497.   /* Determine the progress of the iteration */
  498.  
  499.   state->nslow1++;
  500.   if (actred >= p001)
  501.     state->nslow1 = 0;
  502.  
  503.   if (actred >= p1)
  504.     state->nslow2 = 0;
  505.  
  506.   if (state->ncfail == 2)
  507.     {
  508.       {
  509.         int status = GSL_MULTIROOT_FN_EVAL_DF (fdf, x, J);
  510.         
  511.         if (status != GSL_SUCCESS) 
  512.           {
  513.             return GSL_EBADFUNC;
  514.           }
  515.       }
  516.  
  517.       state->nslow2++;
  518.  
  519.       if (state->iter == 1)
  520.     {
  521.           if (scale)
  522.             compute_diag (J, diag);
  523.       state->delta = compute_delta (diag, x);
  524.     }
  525.       else
  526.         {
  527.           if (scale)
  528.             update_diag (J, diag);
  529.         }
  530.  
  531.       /* Factorize J into QR decomposition */
  532.  
  533.       gsl_linalg_QR_decomp (J, tau);
  534.       gsl_linalg_QR_unpack (J, tau, q, r);
  535.       return GSL_SUCCESS;
  536.     }
  537.  
  538.   /* Compute qtdf = Q^T df, w = (Q^T df - R dx)/|dx|,  v = D^2 dx/|dx| */
  539.  
  540.   compute_qtf (q, df, qtdf);
  541.  
  542.   compute_wv (qtdf, rdx, dx, diag, pnorm, w, v);
  543.  
  544.   /* Rank-1 update of the jacobian Q'R' = Q(R + w v^T) */
  545.  
  546.   gsl_linalg_QR_update (q, r, w, v);
  547.  
  548.   /* No progress as measured by jacobian evaluations */
  549.  
  550.   if (state->nslow2 == 5)
  551.     {
  552.       return GSL_ENOPROGJ;
  553.     }
  554.  
  555.   /* No progress as measured by function evaluations */
  556.  
  557.   if (state->nslow1 == 10)
  558.     {
  559.       return GSL_ENOPROG;
  560.     }
  561.  
  562.   return GSL_SUCCESS;
  563. }
  564.  
  565.  
  566. static void
  567. hybridj_free (void *vstate)
  568. {
  569.   hybridj_state_t *state = (hybridj_state_t *) vstate;
  570.  
  571.   gsl_vector_free (state->v);
  572.   gsl_vector_free (state->w);
  573.   gsl_vector_free (state->rdx);
  574.   gsl_vector_free (state->qtdf);
  575.   gsl_vector_free (state->df);
  576.   gsl_vector_free (state->f_trial);
  577.   gsl_vector_free (state->x_trial);
  578.   gsl_vector_free (state->gradient);
  579.   gsl_vector_free (state->newton);
  580.   gsl_vector_free (state->qtf);
  581.   gsl_vector_free (state->diag);
  582.   gsl_vector_free (state->tau);
  583.   gsl_matrix_free (state->r);
  584.   gsl_matrix_free (state->q);
  585. }
  586.  
  587. static const gsl_multiroot_fdfsolver_type hybridj_type =
  588. {
  589.   "hybridj",            /* name */
  590.   sizeof (hybridj_state_t),
  591.   &hybridj_alloc,
  592.   &hybridj_set,
  593.   &hybridj_iterate,
  594.   &hybridj_free
  595. };
  596.  
  597. static const gsl_multiroot_fdfsolver_type hybridsj_type =
  598. {
  599.   "hybridsj",            /* name */
  600.   sizeof (hybridj_state_t),
  601.   &hybridj_alloc,
  602.   &hybridsj_set,
  603.   &hybridsj_iterate,
  604.   &hybridj_free
  605. };
  606.  
  607. const gsl_multiroot_fdfsolver_type *gsl_multiroot_fdfsolver_hybridj = &hybridj_type;
  608. const gsl_multiroot_fdfsolver_type *gsl_multiroot_fdfsolver_hybridsj = &hybridsj_type;
  609.